#ifndef __AHRS_ADVANCE_H_
#define __AHRS_ADVANCE_H_
#include "ahrs.h"

typedef struct _ahrs_adv
{
    arm_matrix_instance_f32 P;
		arm_matrix_instance_f32 Q;
		arm_matrix_instance_f32 R;
		arm_matrix_instance_f32 K;
		arm_matrix_instance_f32 S;
		arm_matrix_instance_f32 F;
		arm_matrix_instance_f32 FT;
		arm_matrix_instance_f32 L;
		arm_matrix_instance_f32 LT;
		arm_matrix_instance_f32 H;
		arm_matrix_instance_f32 HT;
		arm_matrix_instance_f32 X;
		arm_matrix_instance_f32 Y;

		arm_matrix_instance_f32 tmpP;
		arm_matrix_instance_f32 tmpQ;
		arm_matrix_instance_f32 tmpQ2;
		arm_matrix_instance_f32 tmpX;
		arm_matrix_instance_f32 tmpYX;
		arm_matrix_instance_f32 tmpXY;
		arm_matrix_instance_f32 tmpXX;
		arm_matrix_instance_f32 tmpXXT;
		arm_matrix_instance_f32 tmpS;
		
		int _initialized;
		INT32 init_cnt;
		INT32 calc_cnt;
}ahrs_adv;

extern ahrs_adv ahrs_atti;

void ahrsAdvanceInit(ahrs_adv *ahrs_t,vector3f *Acc,vector3f *gyro, vector3f *Mags);
void ahrsAdvanceUpdate(ahrs_adv *ahrs_t,ahrs_str *atti,vector3f *Acc, vector3f *Gyro, vector3f *Mags, float dt);

#endif

